Program Listing for File map_publisher.h

↰ Return to documentation for file (codes/cubicle_merge/map_publisher.h)

/*******************************************************
 * Copyright (C) 2019, Robotics Group, Nanyang Technology University
 *
 * \file map_publisher.h
 * \author Zhang Handuo (hzhang032@e.ntu.edu.sg)
 * \date Januarary 2017
 * \brief Final step of publishing obstacles on RVIZ.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 *******************************************************/

#ifndef PROJECT_MAP_PUBLISHER_H
#define PROJECT_MAP_PUBLISHER_H
#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>
#include "obstacle_ind_merge.h"
#include "obstacle_merge.h"
#include "util.h"
#include <Eigen/Dense>

//namespace rvt = rviz_visual_tools;

class MapPublisher{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    MapPublisher(ros::NodeHandle, const ros::NodeHandle&,
            obstacle_ind_merge *, Obstacle_merge*, bool local);

    void Refresh(std_msgs::Header &header);

    void timerCallback(const ros::TimerEvent&);

    void PublishCurrentCamera(const Eigen::Isometry3d &Tcw, std_msgs::Header &header);

    void SetCurrentCameraPose(const Eigen::Isometry3d &Tc2w);

    void SetCurrentRoadAngle(double pitch);

    Eigen::Isometry3d GetCurrentCameraPose();

    ros::Timer timer_;

    int sequence_;

private:

    ros::NodeHandle n_local_;

    void PublishObs(const std::map<unsigned long int, MapElement* > &mObs, std_msgs::Header &header);

    void PublishObs_wide(const std::map<unsigned long int, MapElement* > &mObs, std_msgs::Header &header);

    void Publish_road_arrow(double, std_msgs::Header &header);

    void publish_trackedObs(const std::vector<TrackedObstacle*>&, std_msgs::Header&);

    void publish_allObs(const std::vector<MapElement*>&, std_msgs::Header&);

    void publishLabelHelper(const Eigen::Affine3d& pose, const std::string& label);

    void publishPoints(const std::vector<std::vector<Eigen::Vector3d>>& points,
            ros::Publisher& pub, const Eigen::Vector3i& color);

    bool isCamUpdated();
    void ResetCamFlag();
    bool updateParams();

    // individual obstacle merge
    obstacle_ind_merge *mpObstacle_ind_merge;
    // obstacles merge
    Obstacle_merge *mpObstacle_merge;

    ros::Publisher wide_publisher, long_publisher, road_publisher, lane_helper_publisher,
    lane_publisher, curb_publisher, cam_publisher, long_text_publisher;
    std::ofstream file_speed;

    visualization_msgs::Marker vel_arrow;
    visualization_msgs::Marker mCurrentCamera;
    visualization_msgs::Marker mObsPoints;
    visualization_msgs::Marker mObsPoints_t;
    visualization_msgs::Marker mObsPoints_wide;
    visualization_msgs::Marker mObsPoints_wide_t;
    visualization_msgs::Marker mRoadArrow;
    visualization_msgs::Marker mRoadDisk;
    visualization_msgs::Marker mRoadArrow_t;
    visualization_msgs::Marker mRangeHelper, mRangeHelper2;
    std::vector<visualization_msgs::Marker> mRoad_dataset;
    std::vector<visualization_msgs::Marker> mRoad_disk_dataset;
    visualization_msgs::MarkerArray mObsBatch;
    visualization_msgs::MarkerArray mObsBatch_text;
    visualization_msgs::MarkerArray mObsBatch_wide;
    visualization_msgs::MarkerArray mRoadArrows;

    float fCameraSize;
    float fPointSize;

    Eigen::Isometry3d new_pose;
    Eigen::Matrix3d rot_imu2cam;
    bool mbCameraUpdated;
    bool roadMap_publish;
    bool local_frame_;

    double road_pitch;
    double pitch_degree;

    std::string map_frame_id;
    std::string local_frame_id;
    std::string publish_obs_topic;
    std::string publish_obs_text_topic;
    std::string publish_lane_topic;
    std::string publish_curb_topic;
};



#endif //PROJECT_MAP_PUBLISHER_H